#include "Encoder_Task.h"

Chassis_Control_t chassis_;

static void Chassis_Init(void);  // 初始化电机相关参数和遥控器指针
static void Chassis_Set_Control(void);

void Start_Encoder_Task(void const *argument) {
  uint32_t wait_time_ = xTaskGetTickCount();  // 初始化为当前时间戳
  int16_t pwm = 0;
  Chassis_Init();
  while (1) {
    for (size_t i = 0; i < 4; i++) {
      chassis_.motor[i].EncoderUpdate(&chassis_.motor[i]);
    }
    /*需要加一个延时将 currentCount 和 lastCount 的值更新分开，不然delta计算会变成0，不知道什么机理，
    问gpt说freertos计算的是按行的，理论上没问题，但是实际就是不行*/
    HAL_Delay(1);
    Chassis_Set_Control();
    for (size_t i = 0; i < 4; i++) {
      chassis_.motor[i].SpeedGet(&chassis_.motor[i]);
      chassis_.motor[i].Calc(&chassis_.motor[i]);
      pwm = (chassis_.x == 0 && chassis_.y == 0 && chassis_.w == 0) ? 0 : chassis_.motor[i].pid.out;
      chassis_.motor[i].Driver(&chassis_.motor[i], pwm);
    }
    osDelayUntil(&wait_time_, POLL_INTERVAL_MS);
  }
}

static void Chassis_Init(void) {
  Motor_Init(&chassis_.motor[0], &htim2, &htim8, TIM_CHANNEL_1, MotorA_1_GPIO_Port, MotorA_1_Pin, MotorA_2_GPIO_Port,
             MotorA_2_Pin);
  Motor_Init(&chassis_.motor[1], &htim3, &htim8, TIM_CHANNEL_2, MotorB_1_GPIO_Port, MotorB_1_Pin, MotorB_2_GPIO_Port,
             MotorB_2_Pin);
  Motor_Init(&chassis_.motor[2], &htim4, &htim8, TIM_CHANNEL_3, MotorC_1_GPIO_Port, MotorC_1_Pin, MotorC_2_GPIO_Port,
             MotorC_2_Pin);
  Motor_Init(&chassis_.motor[3], &htim5, &htim8, TIM_CHANNEL_4, MotorD_1_GPIO_Port, MotorD_1_Pin, MotorD_2_GPIO_Port,
             MotorD_2_Pin);
  chassis_.motor[0].PidInit(&chassis_.motor[0], 1, 7200, 7200, 800, 10, 20);
  chassis_.motor[1].PidInit(&chassis_.motor[1], 1, 7200, 7200, -800, -10, -20);
  chassis_.motor[2].PidInit(&chassis_.motor[2], 1, 7200, 7200, 800, 10, 20);
  chassis_.motor[3].PidInit(&chassis_.motor[3], 1, 7200, 7200, -800, -10, -20);

  chassis_.chassis_RC = get_remote_control_point();
}

static void Chassis_Set_Control(void) {
  chassis_.x = chassis_.chassis_RC->rocker[0].x_position * LESSEN_FACTOR;
  chassis_.y = chassis_.chassis_RC->rocker[0].y_position * LESSEN_FACTOR;
  chassis_.w = chassis_.chassis_RC->rocker[1].x_position * LESSEN_FACTOR;
  chassis_.motor[0].speed_set = chassis_.x + chassis_.y + chassis_.w;
  chassis_.motor[1].speed_set = -chassis_.x + chassis_.y + chassis_.w;
  chassis_.motor[2].speed_set = -chassis_.x - chassis_.y + chassis_.w;
  chassis_.motor[3].speed_set = chassis_.x - chassis_.y + chassis_.w;
}
